/*
 * @Author: cooper
 * @Date: 2025-04-03 23:26:28
 * @Wechat: cmd_develop
 * @Description:
 */
#include "bsp_step.h"
#include <stdlib.h>
TaskHandle_t step_task_handler;
QueueHandle_t step_work_queue = 0; // step步骤工作队列

u8 cal_flag = 0;
/// @brief 系统初始化
/// @param
void sys_init(void)
{
}

/// @brief 系统校验
/// @param
void sys_cal(void)
{
}

/// @brief 机械臂校验
/// @param
void arm_cal(void)
{
    static u8 step_status = 1;
    u8 code[3] = {0, 0, 0};
    switch (step_status)
    {
    case 1:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;         // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_1;        // 将 REG_POS_1 赋给 code[1]
            code[2] = LONG_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                logPrintln("step_status = 2");

                step_status = 2;
            }
        }
        break;
    case 2:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;          // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_1;         // 将 REG_POS_1 赋给 code[1]
            code[2] = SHORT_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                logPrintln("step_status = 3");

                step_status = 3;
            }
        }
        break;
    case 3:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;      // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_0;     // 将 REG_POS_1 赋给 code[1]
            code[2] = Z_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                logPrintln("step_status = 4");

                step_status = 4;
            }
        }
        break;
    case 4:
        if (LIFT_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;            // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_2;           // 将 REG_POS_1 赋给 code[1]
            code[2] = UPDOWN_LIFT_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            lift_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                logPrintln("step_status = 5");
                step_status = 5;
            }
        }
        break;
    case 5:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;         // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_12;       // 将 REG_POS_1 赋给 code[1]
            code[2] = LONG_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                step_status = 6;
            }
        }
        break;
    case 6:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;         // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_5;        // 将 REG_POS_1 赋给 code[1]
            code[2] = LONG_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                step_status = 7;
            }
        }
        break;
    case 7:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;          // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_1;         // 将 REG_POS_1 赋给 code[1]
            code[2] = STICK_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                step_status = 8;
            }
        }
        break;
    case 8:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;          // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_0;         // 将 REG_POS_1 赋给 code[1]
            code[2] = STICK_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                step_status = 9;
            }
        }
        break;
    case 9:
        if (ARM_REG[REG_STATUS] == MOTO_QUEUE)
        {
            code[0] = MOTO_POS;      // 将 MOTO_POS 赋给 code[0]
            code[1] = REG_POS_1;     // 将 REG_POS_1 赋给 code[1]
            code[2] = Z_ARM_ADDRESS; // 将 LONG_ARM_ADDRESS 赋给 code[2]
            arm_cmd_queue_send(code);
            if (xQueueReceive(step_work_queue, &step_status, portMAX_DELAY) == pdTRUE)
            {
                step_status = 10;
            }
        }
        break;
    default:
        cal_flag = 0;
        logPrintln("finish");
        break;
    }
}

void STEP_TEST(void)
{
    cal_flag = 1;
}

u8 STEP_ARM(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    if (argc == 3)
    {
        // 没做数字判别了.直接转数字了
        u16 addr = atoi(argv[1]); // 将命令行第一个参数转换为整数
        int pos = atoi(argv[2]);  // 将命令行第二个参数转换为整数

        u16 speed;
        u8 acc;
        u32 relAxis = 0;
        logPrintln("地址位置: %d %d\n", addr, pos);
        if (addr == LONG_ARM_ADDRESS)
        {
            speed = LONG_ARM_REG[REG_SPEED];
            acc = LONG_ARM_REG[REG_ACC];
            //+4的原因是前面有REG_STATUS REG_SPEED REG_ACC REG_ERROR
            relAxis = LONG_ARM_REG[pos + 4];
        }
        else if (addr == SHORT_ARM_ADDRESS)
        {
            speed = SHORT_ARM_REG[REG_SPEED];
            acc = SHORT_ARM_REG[REG_ACC];
            relAxis = SHORT_ARM_REG[pos + 4];
        }
        else if (addr == Z_ARM_ADDRESS)
        {
            speed = Z_ARM_REG[REG_SPEED];
            acc = Z_ARM_REG[REG_ACC];
            relAxis = Z_ARM_REG[pos + 4];
        }
        else if (addr == PAW_ARM_ADDRESS)
        {
            speed = PAW_ARM_REG[REG_SPEED];
            acc = PAW_ARM_REG[REG_ACC];
            relAxis = PAW_ARM_REG[pos + 4];
        }
        else if (addr == STICK_ARM_ADDRESS)
        {
            speed = STICK_ARM_REG[REG_SPEED];
            acc = STICK_ARM_REG[REG_ACC];
            relAxis = STICK_ARM_REG[pos + 4];
        }
        else
        {
            logPrintln("格式错误STEP_ARM 1 2,1是电机地址,2是电机位置点");
            return 0;
        }
        TX_arm_TOTAL = moto_run_rel_function(&(TX_arm(0)), addr, speed, acc, relAxis);
        logPrintln(" TX_arm_TOTAL= %d\n", TX_arm_TOTAL);
        for (int i = 0; i <= TX_arm_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, arm_Buffer.tx_data[i]);
        }
        ARM_UART_DMA_TX(TX_arm_TOTAL);
    }
    else
    {
        logPrintln("格式错误STEP_ARM 1 2,1是电机地址,2是电机位置点");
    }
    return 0;
}

u8 STEP_ARM_POS(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    u8 addr;
    u16 pos1, pos2;
    u16 speed;
    u8 acc;
    u32 relAxis = 0;
    if (argc == 4)
    {
        addr = atoi(argv[1]); // 将命令行第一个参数转换为整数
        pos1 = atoi(argv[2]); // 将命令行第二个参数转换为整数
        pos2 = atoi(argv[3]); // 将命令行第二个参数转换为整数
    }

    logPrintln("command eeror:STEP_ARM 1 POS1 POS2,1 addr,2 high regsister,3 low regsister");
    if (addr == LONG_ARM_ADDRESS)
    {
        speed = LONG_ARM_REG[REG_SPEED];
        acc = LONG_ARM_REG[REG_ACC];
    }
    else if (addr == SHORT_ARM_ADDRESS)
    {
        speed = SHORT_ARM_REG[REG_SPEED];
        acc = SHORT_ARM_REG[REG_ACC];
    }
    else if (addr == Z_ARM_ADDRESS)
    {
        speed = Z_ARM_REG[REG_SPEED];
        acc = Z_ARM_REG[REG_ACC];
    }
    else if (addr == PAW_ARM_ADDRESS)
    {
        speed = PAW_ARM_REG[REG_SPEED];
        acc = PAW_ARM_REG[REG_ACC];
    }
    else if (addr == STICK_ARM_ADDRESS)
    {
        speed = STICK_ARM_REG[REG_SPEED];
        acc = STICK_ARM_REG[REG_ACC];
    }
    relAxis = (pos1 << 16) | pos2;
    TX_arm_TOTAL = moto_run_rel_function(&(TX_arm(0)), addr, speed, acc, relAxis);
    logPrintln(" TX_arm_TOTAL= %d\n", TX_arm_TOTAL);
    for (int i = 0; i <= TX_arm_TOTAL - 1; i++)
    {
        logPrintln("i:%d:%x\t", i, arm_Buffer.tx_data[i]);
    }
    ARM_UART_DMA_TX(TX_arm_TOTAL);
}

u8 STEP_LIFT(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    if (argc == 3)
    {
        // 没做数字判别了.直接转数字了
        u16 addr = atoi(argv[1]); // 将命令行第一个参数转换为整数
        int pos = atoi(argv[2]);  // 将命令行第二个参数转换为整数

        u16 speed;
        u8 acc;
        u32 relAxis = 0;
        logPrintln("地址位置: %d %d\n", addr, pos);
        if (addr == UPDOWN_LIFT_ADDRESS)
        {
            speed = UPDOWN_LIFT_REG[REG_SPEED];
            acc = UPDOWN_LIFT_REG[REG_ACC];
            relAxis = UPDOWN_LIFT_REG[pos + 4];
        }
        else if (addr == DOOR_LIFT_ADDRESS)
        {
            speed = DOOR_LIFT_REG[REG_SPEED];
            acc = DOOR_LIFT_REG[REG_ACC];
            relAxis = DOOR_LIFT_REG[pos + 4];
        }
        else
        {
            logPrintln("格式错误STEP_LIFT 1 2,1是电机地址,2是电机位置点");
            return 0;
        }
        TX_LIFT_TOTAL = moto_run_rel_function(&(TX_LIFT(0)), addr, speed, acc, relAxis);
        logPrintln(" TX_LIFT_TOTAL= %d\n", TX_LIFT_TOTAL);
        for (int i = 0; i <= TX_LIFT_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, lift_Buffer.tx_data[i]);
        }
        LIFT_UART_DMA_TX(TX_LIFT_TOTAL);
    }
    else
    {
        logPrintln("格式错误STEP_LIFT 1 2,1是电机地址,2是电机位置点");
    }
    return 0;
}

u8 STEP_LIFT_POS(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    if (argc == 4)
    {
        // 没做数字判别了.直接转数字了
        u16 addr = atoi(argv[1]); // 将命令行第一个参数转换为整数
        u16 pos1 = atoi(argv[2]); // 将命令行第二个参数转换为整数
        u16 pos2 = atoi(argv[3]); // 将命令行第二个参数转换为整数

        u16 speed;
        u8 acc;
        u32 relAxis = 0;

        if (addr == UPDOWN_LIFT_ADDRESS)
        {
            speed = UPDOWN_LIFT_REG[REG_SPEED];
            acc = UPDOWN_LIFT_REG[REG_ACC];
        }
        else if (addr == DOOR_LIFT_ADDRESS)
        {
            speed = DOOR_LIFT_REG[REG_SPEED];
            acc = DOOR_LIFT_REG[REG_ACC];
        }

        relAxis = (pos1 << 16) | pos2;
        TX_LIFT_TOTAL = moto_run_rel_function(&(TX_LIFT(0)), addr, speed, acc, relAxis);
        logPrintln(" TX_LIFT_TOTAL= %d\n", TX_LIFT_TOTAL);
        for (int i = 0; i <= TX_LIFT_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, lift_Buffer.tx_data[i]);
        }
        LIFT_UART_DMA_TX(TX_LIFT_TOTAL);
    }
    else
    {
        logPrintln("COMMAND ERROR,LIKE THIS:STEP_LIFT ADDR POS1 POS2");
    }
    return 0;
}

u8 STEP_FOOD(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    if (argc == 2)
    {
        // 没做数字判别了.直接转数字了
        u16 addr = atoi(argv[1]); // 将命令行第一个参数转换为整数

        u16 speed;
        u8 acc;
        u32 relAxis = 0;
        logPrintln("地址位置: %d\n", addr);
        if (addr == ONE_FOOD_ADDRESS)
        {
            speed = ONE_FOOD_REG[REG_SPEED];
            acc = ONE_FOOD_REG[REG_ACC];
            relAxis = ONE_FOOD_REG[REG_POS_0];
        }
        else if (addr == TWO_FOOD_ADDRESS)
        {
            speed = TWO_FOOD_REG[REG_SPEED];
            acc = TWO_FOOD_REG[REG_ACC];
            relAxis = TWO_FOOD_REG[REG_POS_0];
        }
        else if (addr == THREE_FOOD_ADDRESS)
        {
            speed = THREE_FOOD_REG[REG_SPEED];
            acc = THREE_FOOD_REG[REG_ACC];
            relAxis = THREE_FOOD_REG[REG_POS_0];
        }
        else if (addr == FOUR_FOOD_ADDRESS)
        {
            speed = FOUR_FOOD_REG[REG_SPEED];
            acc = FOUR_FOOD_REG[REG_ACC];
            relAxis = FOUR_FOOD_REG[REG_POS_0];
        }
        else
        {
            logPrintln("COMMAND ERROR,LIKE THIS:STEP_LIFT 1 2,1是电机地址,2是电机位置点");
            return 0;
        }

        TX_food_TOTAL = moto_run_abs_function(&(TX_food(0)), addr, speed, acc, relAxis);
        logPrintln(" TX_food_TOTAL= %d\n", TX_food_TOTAL);
        for (int i = 0; i <= TX_food_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, food_Buffer.tx_data[i]);
        }
        FOOD_UART_DMA_TX(TX_food_TOTAL);
    }
    else
    {
        logPrintln("COMMAND ERROR,LIKE THIS:STEP_FOOD 1 ,1是电机地址");
    }
    return 0;
}
u8 STEP_FOOD_POS(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    if (argc == 4)
    {
        // 没做数字判别了.直接转数字了
        u16 addr = atoi(argv[1]); // 将命令行第一个参数转换为整数
        u16 pos1 = atoi(argv[2]); // 将命令行第二个参数转换为整数
        u16 pos2 = atoi(argv[3]); // 将命令行第二个参数转换为整数
        u16 speed;
        u8 acc;
        u32 relAxis = 0;
        logPrintln("地址位置: %d\n", addr);
        if (addr == ONE_FOOD_ADDRESS)
        {
            speed = ONE_FOOD_REG[REG_SPEED];
            acc = ONE_FOOD_REG[REG_ACC];
        }
        else if (addr == TWO_FOOD_ADDRESS)
        {
            speed = TWO_FOOD_REG[REG_SPEED];
            acc = TWO_FOOD_REG[REG_ACC];
        }
        else if (addr == THREE_FOOD_ADDRESS)
        {
            speed = THREE_FOOD_REG[REG_SPEED];
            acc = THREE_FOOD_REG[REG_ACC];
        }
        else if (addr == FOUR_FOOD_ADDRESS)
        {
            speed = FOUR_FOOD_REG[REG_SPEED];
            acc = FOUR_FOOD_REG[REG_ACC];
        }
        relAxis = (pos1 << 16) | pos2;
        TX_food_TOTAL = moto_run_abs_function(&(TX_food(0)), addr, speed, acc, relAxis);
        logPrintln(" TX_food_TOTAL= %d\n", TX_food_TOTAL);
        for (int i = 0; i <= TX_food_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, food_Buffer.tx_data[i]);
        }
        FOOD_UART_DMA_TX(TX_food_TOTAL);
    }
    else
    {
        logPrintln("COMMAND ERROR,LIKE THIS:STEP_FOOD ADDR POS1 POS2");
    }
    return 0;
}

void STEP_TEMP(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    if (argc == 5)
    {
        // 没做数字判别了.直接转数字了
        u16 addr = atoi(argv[1]);       // 将命令行第一个参数转换为整数
        u16 temp_value = atoi(argv[2]); // 将命令行第一个参数转换为整数
        logPrintln("设置温控地置: %d %d\n", addr, temp_value);

        BP_CONFIG[addr * 3].bake_temputure = atoi(argv[2]);
        BP_CONFIG[addr * 3 + 1].bake_temputure = atoi(argv[3]);
        BP_CONFIG[addr * 3 + 2].bake_temputure = atoi(argv[4]);

        TX_temputure_TOTAL = MASTER_MODBUS_10_CODE(&(TX_temputure(0)), addr, 20, 3);
        logPrintln(" TX_temputure_TOTAL= %d\n", TX_temputure_TOTAL);
        for (int i = 0; i <= TX_temputure_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, temputure_Buffer.tx_data[i]);
        }
        TEMPUTURE_UART_DMA_TX(TX_temputure_TOTAL);
    }
    else
    {
        logPrintln("command error.STEP_TEMP 1 10 20 30,set slave=1 pipi 10 20 30");
    }
}

void STEP_SERVO(int argc, char *argv[])
{
    logPrintln("argc = %d", argc);
    if (argc == 2)
    {
        u16 addr = atoi(argv[1]); // 将命令行第一个参数转换为整数
        tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_2, addr);
        tmr_output_channel_buffer_enable(TMR4, TMR_SELECT_CHANNEL_2, TRUE);
    }
    else
    {
        logPrintln("command error.STEP_SERVO 2500");
    }
}

void STEP_DOOR_ZERO(void)
{
    TX_LIFT_TOTAL = moto_run_home_function(&(TX_LIFT(0)), 0x02);
    logPrintln(" TX_lift_TOTAL= %d\n", TX_LIFT_TOTAL);
    for (int i = 0; i <= TX_LIFT_TOTAL - 1; i++)
    {
        logPrintln("i:%d:%x\t", i, lift_Buffer.tx_data[i]);
    }
    LIFT_UART_DMA_TX(TX_LIFT_TOTAL);
}

void step_task_function(void)
{
    while (1)
    {
        if (cal_flag == 1)
        {
            arm_cal();

            // cal_flag = 0;
        }
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}

void STEP_TASK(void)
{
    step_work_queue = xQueueCreate((UBaseType_t)1,  /* 消息队列的长度 */
                                   (UBaseType_t)1); /* 消息的大小 */
    // 创建接收任务
    xTaskCreate((TaskFunction_t)step_task_function,
                (const char *)"STEP_TASK",
                (uint16_t)configMINIMAL_STACK_SIZE, // 128
                (void *)NULL,
                (UBaseType_t)4,
                (TaskHandle_t *)&step_task_handler);
}
